RANSAC Plane Fitting

The RANSAC algorithm mainly involves performing two iteratively repeated steps on a given data set: Hypothesis and Verification. First, a hypothetical shape of the desired model is generated by randomly selecting a minimal subset of n-points and estimating the corresponding shape-model parameters.

A minimal subset contains the smallest number of points required to uniquely estimate a model. For example, 2 points are needed to determine a line, while 3 non-collinear points are needed to determine a plane as described below.

Let's let p_1,
p_2, and
p_3 be the three randomly selected non-collinear points such that:

p_1=(x_1, y_1, z_1)
p_2=(x_2, y_2, z_2)
p_3=(x_3, y_3, z_3)

A plane can then be described by an equation of the form:

a x + b y + c z + d = 0.

The coefficients a,b,c,d can then be obtained by solving the following system of equations:

ax_1 + by_1 + cz_1 + d = 0
ax_2 + by_2 + cz_2 + d = 0
ax_3 + by_3 + cz_3 + d = 0

This system can be solved using Cramer's rule and some basic matrix manipulations in the following way:

If D is non-zero (so for planes not through the origin) the values for a, b, and c can be calculated as follows:

Once a model is established, the remaining points in the point cloud are tested against the resulting candidate shape to determine how many of the points are well approximated by the model.

After a certain number of iterations, the shape that possesses the largest percentage of inliers is extracted and the algorithm continues to process the remaining data.

Using RANSAC with PCL:

Thankfully, you don't need to implement RANSAC plane fitting yourself because the algorithm already exists in the PCL library! To implement RANSAC plane fitting in your code for this exercise, add the following code to the RANSAC plane segmentation section of your RANSAC.py file:

# Create the segmentation object
seg = cloud_filtered.make_segmenter()

# Set the model you wish to fit 
seg.set_model_type(pcl.SACMODEL_PLANE)
seg.set_method_type(pcl.SAC_RANSAC)

# Max distance for a point to be considered fitting the model
# Experiment with different values for max_distance 
# for segmenting the table
max_distance = 1
seg.set_distance_threshold(max_distance)

# Call the segment function to obtain set of inlier indices and model coefficients
inliers, coefficients = seg.segment()